#include <new>
#include <memory>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <ctime>
#include <boost/shared_array.hpp>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <libplayerc++/playerc++.h>
#include "MapStateValidityChecker.h"
#include "pause.h"
#include "ptRRT.h"
#include "VisualizerImpl.h"
#include "VtExecutor.h"

using namespace PlayerCc;
using namespace ompl::base;
using namespace ompl::geometric;

class Controller {

    boost::random::random_device rng;
    boost::random::uniform_int_distribution<> coin;
    
    // player parameters
    PlayerClient    robot;
    SimulationProxy sim;
    MapProxy        map;
    Position2dProxy pp;
    Position2dProxy gocon;
    Graphics2dProxy g;
    
    // ompl parameters
    StateSpacePtr space;
    SimpleSetup ss;
    
    // standby pos is robot start
    double standbyX;
    double standbyY;
        
    double reachDist;
    
public:
    
    Controller() :
    coin(0,1),
    robot("localhost"),
    sim(&robot, 0),
    map(&robot, 0),
    pp(&robot, 0),
    gocon(&robot, 100),
    g(&robot, 0),
    space(new SE2StateSpace()),
    ss(space),
    reachDist(0.5)
    {
        robot.Read();
        map.RequestMap();
        
        // set bounds
        double xmax = map.GetWidth() * map.GetResolution() / 2.0;
        double ymax = map.GetHeight() * map.GetResolution() / 2.0;
        RealVectorBounds bounds(2);
        bounds.setLow(0, -xmax); bounds.setHigh(0, xmax);
        bounds.setLow(1, -ymax); bounds.setHigh(1, ymax);
        space->as<SE2StateSpace>()->setBounds(bounds);

        // set validity checker
        StateValidityCheckerPtr checker(new MapStateValidityChecker(0.4, ss.getSpaceInformation(), map));
        ss.setStateValidityChecker(checker);
        
        // standby position is start pos
        standbyX = pp.GetXPos();
        standbyY = pp.GetYPos();
        std::cout << "Standby Pos(" << standbyX << ", " << standbyY << ")" <<std::endl;
        
        // get tables and init order sim
    }
    
private:
    
    PlannerPtr getPlanner() {
        PlannerPtr planner(new RRT(ss.getSpaceInformation()));
        planner->as<RRT>()->setRange(0.5);
        return planner;
    }
    
    // plan motion from one start to one goal and execute
    void planAndExecute(const std::string & description, double goalX, double goalY) {
        // get current robot positions etc.
        robot.Read();
        
        // start at the existing position
        ScopedState<SE2StateSpace> start(ss.getSpaceInformation());
        start->setX(pp.GetXPos()); start->setY(pp.GetYPos());
        
        // goal is the standby position
        ScopedState<SE2StateSpace> goal(ss.getSpaceInformation());
        goal->setX(goalX); goal->setY(goalY);
        
        // visualize
        std::cout << description.c_str() << " (" << goalX << ", " << goalY << ")" << std::endl;
        
        // set start and goal
        ss.setStartAndGoalStates(start, goal);
        
        // setup the path planner
        ss.setPlanner(getPlanner());
        ss.clear();
        ss.setup();
        
        // plan
        PlannerStatus solved = ss.solve();
        
        // execute
        if (solved) {
            ss.simplifySolution();
            //record_states(description);
            VtExecutor executor(ss.getSolutionPath(), robot, pp, gocon);
            executor.execute();
            util::pause(5);
        }
        
        std::cout << description.c_str() << " (" << goalX << ", " << goalY << ") " << (solved? "done": "failed!") << std::endl;
        
        
    }

public:

  // control loop
  void run() {
    for (;;) {
      planAndExecute("Point", 5.0, 2.0);
      planAndExecute("Point", 2, 2.0);
    }
  }
};

int
main(int argc, char *argv[])
{
  try {
    Controller control;
    control.run();
    return 0;
  } catch (std::exception &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}
